
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       Dynamical.c
  * @author     baiyang
  * @date       2021-7-14
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "Dynamical.h"
#include <common/gp_math/gp_mathlib.h>

/*-----------------------------------macro------------------------------------*/
#define InToMile        0.0254f
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
//配置电机参数
void Motor_SetParam(Motor* pMotor, float kv, float _resistance, float _esc_resistance ,float no_load_current,
                    float mot_pwm_min, float mot_pwm_max, float mot_spin_min, float mot_spin_max)
{
    pMotor->_kv = kv;
    pMotor->_no_load_current = no_load_current;
    pMotor->_resistance = _resistance;
    pMotor->_esc_resistance = _esc_resistance;
    pMotor->mot_pwm_min = mot_pwm_min;
    pMotor->mot_pwm_max = mot_pwm_max;
    pMotor->mot_spin_max = mot_spin_max;
    pMotor->mot_spin_min = mot_spin_min;
    pMotor->_max_current = 400.0f;
}

//配置螺旋桨参数
void Prop_SetParam(Prop* pProp, float _diameter_in, float _pitch_in, float Ct, float Cm, float mass)
{
    pProp->_diameter = _diameter_in;
    pProp->_pitch = _pitch_in;
    pProp->_Ct = Ct;
    pProp->_Cm = Cm;
    pProp->_mass = mass;
    pProp->_inertia = 0.0833f * pProp->_mass * SQ(pProp->_diameter);
}

void Dynamical_Init(Motor* pMotor)
{
    pMotor->current = 0.0f;
    pMotor->rpm = 0.0f;
    pMotor->torque = 0.0f;
    pMotor->thrust = 0.0f;
}

//根据螺旋桨模型和电机模型  计算拉力和反扭矩
void Dynamical_Calculate_FM(Motor* pMotor, Prop* pProp, float voltage, float density, float dt)
{
    //计算KT值 电流和扭矩的关系
    float Kt = 1 / ((pMotor->_kv) * (M_2PI / 60));

    //计算内阻损耗电压
    float drop_voltage = (pMotor->_kv * voltage - pMotor->rpm) / (pMotor->_kv);
    //计算电流值
    float current_m = drop_voltage / (pMotor->_esc_resistance + pMotor->_resistance);
    //满足最大电流限制
    current_m = MIN(current_m, pMotor->_max_current);
    float current = current_m - pMotor->_no_load_current;
    current = MAX(0.0f, current);
    //计算输出扭矩力
    pMotor->torque = Kt * current;
    float D4 = SQ(pProp->_diameter) * SQ(pProp->_diameter);
    float D5 = D4 * pProp->_diameter;
    
    //计算反扭矩 空气->旋桨
    float prop_drag = pProp->_Cm * density * SQ(pMotor->rpm / 60) * D5;

    float w_rps = pMotor->rpm * RPM_TO_RPS;    //转成rad/s

    //反扭矩动态模型 角加速度更新角速度
    float delta_w = ((pMotor->torque - prop_drag) / pProp->_inertia) * dt;
    w_rps += delta_w;
    w_rps = MAX(w_rps, 0);
    pMotor->thrust = 2.5f * pProp->_Ct * density * SQ(w_rps / M_2PI) * D4;
    //更新电机转数和电流
    pMotor->rpm = w_rps * RPS_TO_RPM;
    pMotor->current = current;
}

//获取拉力
float Motor_GetFoce(Motor* pMotor)
{
    return pMotor->thrust;
}

//获取扭矩
float Motor_GetTorque(Motor* pMotor)
{
    return pMotor->torque;
}

//获取电机输入电流
float Motor_GetInCurrent(Motor* pMotor)
{
    return (pMotor->current + pMotor->_no_load_current);
}

//pwm to (0-1)
float Motor_PwmToCommand(Motor* pMotor, float pwm)
{
    const float pwm_thrust_max = pMotor->mot_pwm_min + pMotor->mot_spin_max * (pMotor->mot_pwm_max - pMotor->mot_pwm_min);
    const float pwm_thrust_min = pMotor->mot_pwm_min + pMotor->mot_spin_min * (pMotor->mot_pwm_max - pMotor->mot_pwm_min);
    const float pwm_range = pwm_thrust_max - pwm_thrust_min;
    return math_constrain_float((pwm - pwm_thrust_min) / pwm_range, 0, 1);
}

/*------------------------------------test------------------------------------*/

